/***************************************************************************************************************************
 * drone_pos_estimator.cpp
 *
 * Author: SFL
 *
 * Update Time: 2020.8.30
 *
 * 说明: 位姿估计程序
 *      1.使用optitrack定位系统创建一个刚体
        2.选择mocap作为位置来源
 *
***************************************************************************************************************************/


//头文件
#include <ros/ros.h>

#include <OptiTrackFeedBackRigidBody.h>
#include <fast_drone_pkg/DroneState.h>
#include <geometry_msgs/PoseStamped.h>
#include <drone_utils.h>

#include <LowPassFilter.h>

using namespace std;
int Use_mocap_raw;
int linear_window;
int angular_window;
float T;//一阶低通滤波器时间常数
float cur_time;
rigidbody_state UAVstate;
ros::Publisher drone_state_pub;
// ros::Publisher mocap_pub;
// ros::Publisher vision_pub;
fast_drone_pkg::DroneState _DroneState;  
//---------------------------------------无人机位置及速度--------------------------------------------
// Eigen::Vector3d pos_drone_fcu;                           //无人机当前位置 (来自fcu)
// Eigen::Vector3d vel_drone_fcu;                           //无人机当前时刻速度 (来自fcu)
// Eigen::Vector3d Att_fcu;                               //无人机当前欧拉角(来自fcu)
// Eigen::Vector3d Att_rate_fcu;

void printf_info();

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "drone_pos_estimator");
    ros::NodeHandle nh("~");

    nh.param("pos_estimator/Use_mocap_raw", Use_mocap_raw, 1);
    nh.param("pos_estimator/linear_window", linear_window, 3);
    nh.param("pos_estimator/angular_window", angular_window,3);
    nh.param<float>("pos_estimator/noise_T", T, 0.02);

    // LowPassFilter LPF_x;
    // LowPassFilter LPF_y;
    // LowPassFilter LPF_z;
    LowPassFilter LPF_roll;
    LowPassFilter LPF_pitch;
    LowPassFilter LPF_yaw;

    // LPF_x.set_Time_constant(T);//设置时间函数
    // LPF_y.set_Time_constant(T);
    // LPF_z.set_Time_constant(T);
    LPF_roll.set_Time_constant(T);
    LPF_pitch.set_Time_constant(T);
    LPF_yaw.set_Time_constant(T);

    OptiTrackFeedBackRigidBody FastDrone("/vrpn_client_node/fastdrone/pose", nh, linear_window, angular_window);

    drone_state_pub = nh.advertise<fast_drone_pkg::DroneState>("/fast_drone/drone_state", 2);

    ros::Rate rate(100.0);//100Hz 所以采样周期是0.01s
    float dt = 0.01;//采样周期是0.01s
    ros::Time begin_time = ros::Time::now();
    float last_time = drone_utils::get_time_in_sec(begin_time);

    while (ros::ok())
    {
        // 当前时间
        cur_time = drone_utils::get_time_in_sec(begin_time);
        dt = cur_time  - last_time;
        dt = constrain_function2(dt, 0.01, 0.02);
        last_time = cur_time;

        ros::spinOnce();

        FastDrone.RosWhileLoopRun();
        FastDrone.GetState(UAVstate);

        // send_to_fcu(UAVstate);

        // 发布无人机状态至drone_pos_controller.cpp节点，根据参数Use_mocap_raw选择位置速度消息来源
        _DroneState.header.stamp = ros::Time::now();   

        //一阶低通滤波器
        // UAVstate.Position(0) = LPF_x.apply(UAVstate.Position(0), dt);
        // UAVstate.Position(1) = LPF_y.apply(UAVstate.Position(1), dt);
        // UAVstate.Position(2) = LPF_z.apply(UAVstate.Position(2), dt);
        UAVstate.Euler(0) = LPF_roll.apply(UAVstate.Euler(0), dt);
        UAVstate.Euler(1) = LPF_pitch.apply(UAVstate.Euler(1), dt);
        UAVstate.Euler(2) = LPF_yaw.apply(UAVstate.Euler(2), dt);

        //飞行器位置状态数据（从optitrack中读出）
        if(Use_mocap_raw == 1)
        {
            for(int i = 0; i < 3; i++)
            {
                _DroneState.position[i] = UAVstate.Position(i);
            }
        }
        else if (Use_mocap_raw == 2)//
        {
            for(int i = 0; i < 3; i++)
            {
                _DroneState.position[i] = UAVstate.Position(i);
                _DroneState.velocity[i] = UAVstate.V_I(i);
            }
        }
        // 姿态数据（optitrack）
        for(int i = 0; i < 3; i++)//
        {
            _DroneState.attitude[i] = UAVstate.Euler(i);
            _DroneState.attitude_rate[i] = UAVstate.Omega_BI(i);
        }
        _DroneState.attitude_q.w = UAVstate.quaterion(0);
        _DroneState.attitude_q.x = UAVstate.quaterion(1);
        _DroneState.attitude_q.y = UAVstate.quaterion(2);
        _DroneState.attitude_q.z = UAVstate.quaterion(3);

        drone_state_pub.publish(_DroneState);

        printf_info();
        rate.sleep();
    }
    


    return 0;

}


void printf_info()
{
    // cout <<">>>>>>>>>>>>>>>>>>>>>>>>PX4_POS_ESTIMATOR<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(2);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    //optitrack info   

    cout <<">>>>>>>>>>>>>>>>>>>>>>>>Optitrack origin Info [Fake ENU Frame]<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    cout << "Pos_Optitrack [X Y Z] : " << UAVstate.Position(0) << " [ m ] "<< UAVstate.Position(1) <<" [ m ] "<< UAVstate.Position(2) <<" [ m ] "<<endl;
    cout << "Vel_Optitrack [X Y Z] : " << UAVstate.V_I(0) << " [m/s] "<< UAVstate.V_I(1) <<" [m/s] "<< UAVstate.V_I(2) <<" [m/s] "<<endl;
    cout << "Att_Optitrack [R P Y] : " << UAVstate.Euler(0) * 180/M_PI <<" [deg] "<< UAVstate.Euler(1) * 180/M_PI << " [deg] "<< UAVstate.Euler(2) * 180/M_PI<<" [deg] "<<endl;
    cout << "Attitude_Rate [R P Y] : " << UAVstate.Omega_BI(0) * 180/M_PI <<" [deg] "<< UAVstate.Omega_BI(1) * 180/M_PI << " [deg] "<< UAVstate.Omega_BI(2) * 180/M_PI<<" [deg] "<<endl;

    //using optitrack system

    
}